#include "STLinkV2.h"
#include "SWD.h"

u8 *rwbuff;
static void initpbuff(u8 *pbuff)
{
	rwbuff = pbuff;
}

enum swdcmd
{
	GETSTATUS = 0x01,
	FORCEDEBUG = 0x02,
	APIV1_RESETSYS = 0x03,
	APIV1_READALLREGS = 0x04,
	APIV1_READREG = 0x05,
	APIV1_WRITEREG = 0x06,
	READMEM_32BIT = 0x07,
	WRITEMEM_32BIT = 0x08,
	RUNCORE = 0x09,
	STEPCORE = 0x0a,
	APIV1_SETFP = 0x0b,
	READMEM_8BIT = 0x0c,
	WRITEMEM_8BIT = 0x0d,
	APIV1_CLEARFP = 0x0e,
	APIV1_WRITEDEBUGREG = 0x0f,
	APIV1_SETWATCHPOINT = 0x10,
	ENTER_JTAG_RESET = 0x00,
	ENTER_SWD_NO_RESET = 0xa3,
	ENTER_JTAG_NO_RESET = 0xa4,
	APIV1_ENTER = 0x20,
	EXIT = 0x21,
	READCOREID = 0x22,
	APIV2_ENTER = 0x30,
	APIV2_READ_IDCODES = 0x31,
	APIV2_RESETSYS = 0x32,
	APIV2_READREG = 0x33,
	APIV2_WRITEREG = 0x34,
	APIV2_WRITEDEBUGREG = 0x35,
	APIV2_READDEBUGREG = 0x36,
	APIV2_READALLREGS = 0x3A,
	APIV2_GETLASTRWSTATUS = 0x3B,
	APIV2_DRIVE_NRST = 0x3C,
	APIV2_GETLASTRWSTATUS2 = 0x3E,
	APIV2_START_TRACE_RX = 0x40,
	APIV2_STOP_TRACE_RX = 0x41,
	APIV2_GET_TRACE_NB = 0x42,
	APIV2_SWD_SET_FREQ = 0x43,
	APIV2_JTAG_SET_FREQ = 0x44,
	APIV2_READ_DAP_REG = 0x45,
	APIV2_WRITE_DAP_REG = 0x46,
	APIV2_READMEM_16BIT = 0x47,
	APIV2_WRITEMEM_16BIT = 0x48,
	APIV2_INIT_AP = 0x4B,
	APIV2_CLOSE_AP_DBG = 0x4C,
};

u8 *lastsend;
u16 lastlen;
u8 lastcount;

u32 r4(u8 **i)
{
	u32 v = 0;
	u8 *p = *i;
	_32T8LL(v) = *p++;
	_32T8L(v) = *p++;
	_32T8H(v) = *p++;
	_32T8HH(v) = *p++;
	*i = p;
	return v;
}

u32 r2(u8 **i)
{
	u32 v = 0;
	u8 *p = *i;
	_16T8L(v) = *p++;
	_16T8H(v) = *p++;
	*i = p;
	return v;
}

void cmd(readData rd, writeData wd)
{
	static u8 ack;
	u32 addr;
	u8 i;
	u16 len = 0;
	u32 value;
	u8 *p = rwbuff;
	u8 *output = p + 16;
	rd(p, 4);
	rd(p+4,p[0]);
	switch (p[4])
	{
	case 0xf1:
		((u32 *)output)[0] = 0x0483c728;
		((u32 *)output)[1] = 0x7a563748;
		len = 6;
		break;
	case 0xf5:
		((u32 *)output)[0] = 2;
		len = 2;
		break;
	case 0xf7:
		((u32 *)output)[0] = 0x000005d9;
		((u32 *)output)[1] = 0x00000804;
		len = 8;
		break;
	case 0xf2:
		p += 5;
		switch (*p++)
		{
		case GETSTATUS:
			break;
		case FORCEDEBUG:
			break;
		case APIV1_RESETSYS:
			break;
		case APIV1_READALLREGS:
			break;
		case APIV1_READREG:
			break;
		case APIV1_WRITEREG:
			break;
		case READMEM_32BIT:
			SWD.SetDataLenght(2);
			addr = r4(&p);
			len = r2(&p);
			p = (u8 *)(((u32)p + 4) & -4);
			ack = SWD.ReadMem(addr, len, (u32 *)p);
			output = p + (addr & 3);
			break;
		case WRITEMEM_32BIT:
			SWD.SetDataLenght(2);
			addr = r4(&p);
			len = r2(&p);
			rd(p + (addr & 3), len);
			ack = SWD.WriteMem(addr, len, (u32 *)p);
			len=0;
			//((u32 *)output)[0] = 0x80;
			// len = 1;
			break;
		case RUNCORE:
			break;
		case STEPCORE:
			break;
		case APIV1_SETFP:
			break;
		case READMEM_8BIT:
			SWD.SetDataLenght(0);
			addr = r4(&p);
			len = r2(&p);
			p = (u8 *)(((u32)p + 4) & -4);
			ack = SWD.ReadMem(addr, len, (u32 *)p);
			output = p + (addr & 3);
			break;
		case WRITEMEM_8BIT:
			SWD.SetDataLenght(0);
			addr = r4(&p);
			len = r2(&p);
			rd(p + (addr & 3), len);
			ack = SWD.WriteMem(addr, len, (u32 *)p);
			len=0;
			//((u32 *)output)[0] = 0x80;
			// len = 1;
			break;
		case APIV1_CLEARFP:
			break;
		case APIV1_WRITEDEBUGREG:
			break;
		case APIV1_SETWATCHPOINT:
			break;
		case ENTER_JTAG_RESET:
			break;
		case ENTER_SWD_NO_RESET:
			break;
		case ENTER_JTAG_NO_RESET:
			break;
		case APIV1_ENTER:
			//

			break;
		case EXIT:
			//
			((u32 *)output)[0] = 0x80;
			len = 1;
			break;
		case READCOREID:
			break;
		case APIV2_ENTER:
			((u32 *)output)[0] = SWD.Connect() ? 0x80 : 0;
			len = 2;
			break;
		case APIV2_READ_IDCODES:
			//
			((u32 *)output)[0] = 0x80;
			((u32 *)output)[1] = SWD.IDCores();
			((u32 *)output)[2] = 0;
			len = 12;
			break;
		case APIV2_RESETSYS:
			break;
		case APIV2_READREG:
			addr = *p;
			ack = SWD.GetReg(addr, &value);
			((u32 *)output)[0] = 0x80;
			((u32 *)output)[1] = value;
			len = 8;
			break;
		case APIV2_WRITEREG:
			addr = *p++;
			value = r4(&p);
			ack = SWD.SetReg(addr, value);
			((u32 *)output)[0] = 0x80;
			len = 2;
			break;
		case APIV2_WRITEDEBUGREG:
			addr = r4(&p);
			value = r4(&p);
			ack = SWD.WriteData(addr, value);
			((u32 *)output)[0] = 0x80;
			len = 2;
			break;
		case APIV2_READDEBUGREG:
			addr = r4(&p);
			ack = SWD.ReadData(addr, &value);
			((u32 *)output)[0] = 0x80;
			((u32 *)output)[1] = value;
			len = 8;
			break;
		case APIV2_READALLREGS:
			((u32 *)output)[0] = 0x80;
			for (i = 0; i < 21; i++)
				SWD.GetReg(i, (u32 *)&output[4 * i + 4]);
			len = 88;
			break;
		case APIV2_GETLASTRWSTATUS:
			break;
		case APIV2_DRIVE_NRST:
			((u32 *)output)[0] = 0x80;
			len = 2;
			break;
		case APIV2_GETLASTRWSTATUS2:
			//
			((u32 *)output)[0] = ack ? 0x80 : 0x18;
			((u32 *)output)[1] = ack ? 0 : addr;
			len = 12;
			break;
		case APIV2_START_TRACE_RX:
			break;
		case APIV2_STOP_TRACE_RX:
			break;
		case APIV2_GET_TRACE_NB:
			break;
		case APIV2_SWD_SET_FREQ:
			((u32 *)output)[0] = 0x80;
			len = 2;
			break;
		case APIV2_JTAG_SET_FREQ:
			break;
		case APIV2_READ_DAP_REG:
			addr = r2(&p);
			addr = r2(&p);
			ack = SWD.ReadDAP(addr, &value);
			((u32 *)output)[0] = 0x80;
			((u32 *)output)[1] = value;
			len = 8;
			break;
		case APIV2_WRITE_DAP_REG:
			addr = r2(&p);
			addr = r2(&p);
			value = r4(&p);
			ack = SWD.WriteDAP(addr, value);
			((u32 *)output)[0] = 0x80;
			len = 2;
			break;
		case APIV2_READMEM_16BIT:
			SWD.SetDataLenght(1);
			addr = r4(&p);
			len = r2(&p);
			p = (u8 *)(((u32)p + 4) & -4);
			ack = SWD.ReadMem(addr, len, (u32 *)p);
			output = p + (addr & 3);
			break;
		case APIV2_WRITEMEM_16BIT:
			SWD.SetDataLenght(1);
			addr = r4(&p);
			len = r2(&p);
			rd(p + (addr & 3), len);
			ack = SWD.WriteMem(addr, len, (u32 *)p);
			len=0;
			//((u32 *)output)[0] = 0x80;
			// len = 1;
			break;
		case APIV2_INIT_AP:
			//
			((u32 *)output)[0] = 0x80;
			len = 2;
			break;
		case APIV2_CLOSE_AP_DBG:
			//
			((u32 *)output)[0] = 0x80;
			len = 2;
			break;
		}
		break;
	default:
		break;
	}

	if (len != 0)
	{
		wd(lastsend = output, lastlen = len);
	}
}

const STLinkV2Base STLinkV2 = {
	initpbuff,
	cmd,
};
